//定位相关
#ifndef HITSZ_DIP_PRACTICUM_LOCALIZATION_H
#define HITSZ_DIP_PRACTICUM_LOCALIZATION_H

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include "KMF.h"
#include "detector.h"
#include "param.h"
namespace dip
{
    class Localization
    {
        public:
            Localization()
            {
                this->init_coords_filter();
                this->init_yaw_filter();
                this->coords_filter.print_debug_string();
                this->yaw_filter.print_debug_string();
            }

            /**
             * @brief 初始化坐标滤波器
             * @note 坐标滤波器所有单位都以SI为准,B需要根据实时帧率和R推导
             * X = [x y].T,U = [vcx vcy].T,Z = [x_vision y_vision].T
             * B = w_c_R*dt
             */ 
            void init_coords_filter()
            {
                Eigen::MatrixXf F = Eigen::MatrixXf::Identity(2,2);
                this->coords_filter.setF(F);
                Eigen::MatrixXf H = Eigen::MatrixXf::Identity(2,2);
                this->coords_filter.setH(H);

                Eigen::MatrixXf Q;
                cv::cv2eigen(Param::COORDS_MODEL_COV,Q);
                this->coords_filter.setQ(Q);
                Eigen::MatrixXf R;
                cv::cv2eigen(Param::COORDS_MEASURE_COV,R);
                this->coords_filter.setR(R);
                this->coords_filter.assert_initialized();
            }

            /**
             * @brief 初始化yaw滤波器
             * @note 滤波器单位以SI为准,即弧度+s,B和F都需要根据dt进行设置
             * dt默认为0.3s
             */ 
            void init_yaw_filter()
            {
                Eigen::MatrixXf F = Eigen::MatrixXf(2,2);
                F<<1,0.03,0,1;
                this->yaw_filter.setF(F);
                Eigen::MatrixXf H = Eigen::MatrixXf(1,2);
                H<<1,0;
                this->yaw_filter.setH(H);

                Eigen::MatrixXf Q;
                cv::cv2eigen(Param::YAW_MODEL_COV,Q);
                this->yaw_filter.setQ(Q);
                Eigen::MatrixXf R;
                cv::cv2eigen(Param::YAW_MEASURE_COV,R);
                this->yaw_filter.setR(R);
                this->yaw_filter.assert_initialized();
            }

            /***
             * @brief 3d-3d位姿求解,解出在世界坐标系下相机位姿
             * @param _p_cam_left,左侧圆柱在相机坐标系下的坐标
             * @param _p_cam_right,右侧圆柱在相机坐标系下的坐标
             * @note w_P = w_cR*c_P + w_P_corg,w_P对应q,c_P对应p
             * @note ICP求解网上blog公式不一样，有的是UVT有的是VUT，这个取决于如何构造代码中的S，这里列了参考的代码
             * @note 需要注意的是,numpy和eigen求解结果并不一样，并且numpy直接返回V.T，eigen则是V
             * @note 另外,SVD求解并不唯一，因此需要加入约束条件|R| = 1,这个可以参考最后一个博文
             * @ref 原理&代码参考:https://zhuanlan.zhihu.com/p/107218828
             * @ref |R| = 1的处理:https://zhuanlan.zhihu.com/p/104735380
             * @ref
             */ 
            void ICPSolver(Eigen::Vector2f &_p_cam_left,Eigen::Vector2f &_p_cam_right)
            {
                //计算质心
                Eigen::Vector2f p_cam_c = (_p_cam_left + _p_cam_right)/2.0f;
                Eigen::Vector2f p_world_c = (Param::LEFT_COLUMN_COORDS + Param::RIGHT_COLUMN_COORDS)/2.0f;
                //计算去质心坐标,x = pi - p_c,y = qi - qc,并获得S = X(2x2)*Y.T(2x2)
                //x1 = p1 - p_c = _p_cam_left - p_cam_c
                //y1 = q1 - q_c = Param::LEFT_COLUMN_COORDS - p_world_c
                //S = XY.T = Sigma(xi*yi.t)
                Eigen::Matrix2f S = Eigen::Matrix2f::Zero();
                S += (_p_cam_left - p_cam_c)*(Param::LEFT_COLUMN_COORDS - p_world_c).transpose();
                S += (_p_cam_right - p_cam_c)*(Param::RIGHT_COLUMN_COORDS - p_world_c).transpose();
                // std::cout<<"S:"<<S<<std::endl;

                //SVD分解
                Eigen::JacobiSVD<Eigen::Matrix2f> svd(S,Eigen::ComputeFullU|Eigen::ComputeFullV);
                Eigen::Matrix2f U = svd.matrixU();
                Eigen::Matrix2f V = svd.matrixV();
                // std::cout<<"U="<<U<<",V="<<V<<std::endl;

                Eigen::MatrixXf VUT = V*U.transpose();
                Eigen::Matrix2f M = Eigen::Matrix2f::Identity();
                M(1,1) = VUT.determinant();
                R_w2c = V*M*U.transpose();
                chasis_coords = Param::LEFT_COLUMN_COORDS - R_w2c*_p_cam_left;
                std::cout<<"solve R:"<<this->R_w2c<<std::endl;
                std::cout<<"solve camera coords:"<<this->chasis_coords.transpose()<<std::endl;

            }

            /**
             * @brief 根据旋转矩阵计算yaw
             * @note yaw为弧度制,主值区间为[0,2pi]
             */ 
            inline void calc_yaw()
            {
                float cosine = this->R_w2c(0,0);
                float sine = this->R_w2c(0,1);
                if (fabs(cosine) < 1e-4 && sine >0)
                {
                    this->yaw = 90.0*CV_PI/180.0f;
                }
                else if (fabs(cosine) < 1e-4 && sine <0)
                {
                    this->yaw = 270.0*CV_PI/180.0f;
                }
                else
                {
                    double t = atan2(sine,cosine);//如果cosine不为0,直接用atan2计算,[-pi,pi]
                    this->yaw = t + CV_PI;
                }
            }
            /**
             * @brief 根据相机得到的圆柱尺寸对坐标进行修正
             * @param _p_cam_left,左侧圆柱在相机坐标系下的坐标,单位为m
             * @param _p_cam_right,右侧圆柱在相机坐标系下的坐标,单位为m
             * @note 由于使用imu数据,如果单纯在theta上加减会带来幅度问题,因此在更新时会将测量角度映射到最近的一个区间
             * @note 在更新时会将角度重映射会[0,2pi]
             */ 
            void update(Eigen::Vector2f &_p_cam_left,Eigen::Vector2f &_p_cam_right)
            {
                this->ICPSolver(_p_cam_left,_p_cam_right);
                //计算yaw
                this->calc_yaw();
                if (this->miss_cnt > this->miss_threhold)
                { //如果大于阈值，则根据当前观测重定位
                    this->reset();
                    return ;
                }

                //更新坐标
                // Eigen::MatrixXf B = this->R_w2c*dt;
                // this->coords_filter.setB(B);
                Eigen::VectorXf observe_chasis(2);
                observe_chasis<<this->chasis_coords(0,0),this->chasis_coords(1,0);
                this->coords_filter.update(this->chasis_coords);
                //更新角度
                Eigen::VectorXf x;//yaw的状态变量
                this->yaw_filter.getState(x);
                float nearst_yaw = 0.0f;
                float min_error = 1e+7;
                for (int i=-1;i<=1;i++)
                {
                    float error = fabs(this->yaw - x(0,0) + i*2*CV_PI);
                    if (error < min_error)
                    {
                        min_error = error;
                        nearst_yaw = this->yaw + i*2*CV_PI;
                    }
                }
                Eigen::VectorXf z(1);
                z<<nearst_yaw;
                // std::cout<<"[DEBUG] nearst_yaw:"<<z<<std::endl;
                this->yaw_filter.update(z);
                this->miss_cnt = 0;
            }

            /**
             * @brief 根据控制量进行更新预测
             * @note 对于yaw的滤波器，更新完成后会将状态的第一个值变换到[0,2pi]区间内
             */ 
            bool predict(const Eigen::Vector2f &_velocity,const float _omega,float _dt,Eigen::VectorXf &_coords,float &_yaw)
            {
                //:
                Eigen::VectorXf u(1);
                u<<_omega;
                Eigen::VectorXf x(2);
                Eigen::MatrixXf F(2,2);
                F<<1,_dt,0,1;
                Eigen::MatrixXf B1(2,1);
                B1<<_dt,0;
                this->yaw_filter.setF(F);
                this->yaw_filter.setB(B1);
                bool isinited = this->yaw_filter.predict(u,x);
                
                if (!isinited){return false;}
                //限幅
                if (x(0,0) > 2*CV_PI)
                {
                    x(0,0) -= 2*CV_PI;
                }
                else if (x(0,0) < 0)
                {
                    x(0,0) += 2*CV_PI;
                }
                // std::cout<<"CLAMP"<<x.transpose()<<std::endl;
                // std::cout<<"------end update yaw----"<<std::endl;
                this->yaw_filter.setState(x);
                _yaw = x(0,0);//赋值

                //coords
                Eigen::MatrixXf B2(2,2);
                B2<<cos(x(0,0)),sin(x(0,0)),-sin(x(0,0)),cos(x(0,0));
                B2 *= -1* _dt;
                this->coords_filter.setB(B2);
                isinited = this->coords_filter.predict(_velocity,_coords);
                if (!isinited){return false;}
                this->miss_cnt++;
                return true;
            }

            /**
             * @brief 根据当前定位重置滤波器状态
             */ 
            void reset(Eigen::Vector2f &_p_cam_left,Eigen::Vector2f &_p_cam_right)
            {
                std::cout<<"[Localiazation] RESET with observation"<<std::endl;
                this->ICPSolver(_p_cam_left,_p_cam_right);
                //计算yaw
                this->calc_yaw();
                Eigen::VectorXf x(2);//yaw的状态变量
                x<<this->yaw,0;
                std::cout<<"\t YAW:"<<x.transpose()<<std::endl;
                std::cout<<"\t COORDS:"<<this->chasis_coords.transpose()<<std::endl;
                this->yaw_filter.reset(x);
                this->coords_filter.reset(this->chasis_coords);
                this->miss_cnt = 0;
            }

            /**
             * @brief 根据类状态重置滤波器状态
             */ 
            void reset()
            {
                std::cout<<"[Localiazation] RESET"<<std::endl;
                Eigen::VectorXf x(2);//yaw的状态变量
                x<<this->yaw,0;
                std::cout<<"\t YAW"<<x.transpose()<<std::endl;
                std::cout<<"\t COORDS"<<this->chasis_coords.transpose()<<std::endl;
                this->yaw_filter.reset(x);
                this->coords_filter.reset(this->chasis_coords);
                this->miss_cnt = 0;
            }

            /**
             * @brief 获取解算(观测)的yaw
             */ 
            float get_calc_yaw(){return this->yaw;}
            void get_calc_coords(Eigen::Vector2f &_x){_x = this->chasis_coords;}

            //获取预测(滤波)值
            float get_filter_yaw()
            {
                Eigen::VectorXf x(1);
                this->yaw_filter.getState(x);
                return x(0,0);
            }
            void get_filter_coords(Eigen::Vector2f &_x)
            {
                Eigen::VectorXf tmp(2);
                this->coords_filter.getState(tmp);
                _x<<tmp(0,0),tmp(1,0);
            }

            /**
             * @brief 获取坐标滤波器的方差
             */ 
            void get_coords_cov(float &cov_x,float &cov_y)
            {
                Eigen::MatrixXf Cov;
                this->coords_filter.getCov(Cov);
                cov_x = Cov(0,0);
                cov_y = Cov(1,1);
            }
        private:

        public:
        private:
            Eigen::Matrix2f R_w2c;//世界坐标系到相机坐标系的旋转矩阵
            Eigen::Vector2f chasis_coords;//机器人坐标,这里设置为世界坐标系下相机位姿
            float yaw;//机器人朝向

            unsigned int miss_cnt = 40;//丢失次数
            const int miss_threhold = 20;//丢失阈值,

            KalmanFilter coords_filter;//坐标滤波器
            KalmanFilter yaw_filter;//

    };
}
#endif